/*
  Motor.cpp- Library for controlling two motors with a ardumoto shield.
  *NOTE* Used code given by sparkfun to base library off of.
  created by Matthew Hughes, febuary 18, 2014.
  Released into the public domain.
*/
#include "Arduino.h"
#include "Motor.h"

Motor::Motor(int pwm_a, int pwm_b, int dir_a, int dir_b, int val)
{
  _pwm_a = pwm_a;   //PWM control for motor outputs 1 and 2 is on digital pin 3
  _pwm_b = pwm_b;  //PWM control for motor outputs 3 and 4 is on digital pin 11
  _dir_a = dir_a;  //direction control for motor outputs 1 and 2 is on digital pin 12
  _dir_b = dir_b;  //direction control for motor outputs 3 and 4 is on digital pin 13
  _val = val;     //value for fade
}
void Motor::setup()
{
  pinMode(_pwm_a, OUTPUT);  //Set control pins to be outputs
  pinMode(_pwm_b, OUTPUT);
  pinMode(_dir_a, OUTPUT);
  pinMode(_dir_b, OUTPUT);
  
  //analogWrite(pwm_a, 100);  //set both motors to run at (100/255 = 39)% duty cycle (slow)
  //analogWrite(pwm_b, 100);
}
void Motor::pwm_forw()
{
  digitalWrite(_dir_a, HIGH);  //Reverse motor direction, 1 high, 2 low
  digitalWrite(_dir_b, HIGH);  //Reverse motor direction, 3 low, 4 high
}
void Motor::pwm_back()
{
  digitalWrite(_dir_a, LOW);  //Set motor direction, 1 low, 2 high
  digitalWrite(_dir_b, LOW);  //Set motor direction, 3 high, 4 low
}
void Motor::Full_Forward()
{
  digitalWrite(_dir_a, HIGH);  //Reverse motor direction, 1 high, 2 low
  digitalWrite(_dir_b, HIGH);  //Reverse motor direction, 3 low, 4 high
  analogWrite(_pwm_a, 255);    //set both motors to run at (255/255 = 100)% duty cycle
  analogWrite(_pwm_b, 255);
}
void Motor::Full_Backward()
{
  digitalWrite(_dir_a, LOW);  //Set motor direction, 1 low, 2 high
  digitalWrite(_dir_b, LOW);  //Set motor direction, 3 high, 4 low
  analogWrite(_pwm_a, 255);   //set both motors to run at 100% duty cycle (fast)
  analogWrite(_pwm_b, 255);
}
void Motor::Half_Forward()
{
  digitalWrite(_dir_a, HIGH);  //Reverse motor direction, 1 high, 2 low
  digitalWrite(_dir_b, HIGH);  //Reverse motor direction, 3 low, 4 high
  analogWrite(_pwm_a, 127);    //set both motors to run at (127/255 = 50)% duty cycle
  analogWrite(_pwm_b, 127);
}
void Motor::Half_Backward()
{
  digitalWrite(_dir_a, LOW);  //Set motor direction, 1 low, 2 high
  digitalWrite(_dir_b, LOW);  //Set motor direction, 3 high, 4 low
  analogWrite(_pwm_a, 127);   //set both motors to run at 50% duty cycle (medium)
  analogWrite(_pwm_b, 127);
}
void Motor::Fadein()
{
 // fade in from min to max in increments of 5 points:
  for(int fadeValue = 0 ; fadeValue <= 255; fadeValue +=5) 
  { 
     // sets the value (range from 0 to 255):
    analogWrite(_pwm_a, fadeValue);
    analogWrite(_pwm_b, fadeValue);
    // wait for 30 milliseconds to see the dimming effect    
    delay(30);  
  }
}
void Motor::Fadeout()
{
  for(int fadeValue = 255 ; fadeValue >= 0; fadeValue -=5) 
  { 
    // sets the value (range from 0 to 255):
    analogWrite(_pwm_a, fadeValue);
    analogWrite(_pwm_b, fadeValue);
    // wait for 30 milliseconds to see the dimming effect    
    delay(30);  
  }
}
void Motor::stop_A()
{
  analogWrite(_pwm_a, 0);    //set both motors to run at 0% duty cycle (stop)
}
void Motor::stop_B()
{
  analogWrite(_pwm_b, 0);    //set both motors to run at 0% duty cycle (stop)
}

void Motor::stop_AB()
{
  analogWrite(_pwm_a, 0);    //set both motors to run at 0% duty cycle (stop)
  analogWrite(_pwm_b, 0);    //set both motors to run at 0% duty cycle (stop)
}

